﻿
// 2020/1/10: 首个记录（基于LaneSensorSampleV3.cs 2019/9/27版本修改）
// 2020/1/14: 接口完备性。增加LaneLine.ModelValid/SegmentsValid
// 2020/1/16: 简化描述判定代码。更新指标计算公式
// 2020/5/27: 修正车道线线段ID赋值
// 2020/6/17: public ID改为raw ID
// 2020/7/1: 修正Segments模式插值问题
// 2020/7/20: 更新样本标题
// 2020/9/29: 修正TTLC计算

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using ASEva;

namespace LaneSensorSampleV4
{
    enum LineDescription
    {
        Model = 1, // 通过模型参数和一致的属性进行描述
        Segments = 2, // 通过多段线段和各自的属性进行描述
        Both = 3, // Model和Segments双模式描述
    }

    enum LineClass
    {
        General = 1, // 未指定
        Dashed = 2, // 虚线
        Solid = 3, // 实线
        DoubleDashed = 4, //双虚线
        DoubleSolid = 5, //双实线
        SolidDashed = 6, // 左实右虚
        DashedSolid = 7, // 左虚右实
        Curb = 8, // 路沿
        Guardrail = 9, // 护栏
        Barrier = 10, //障碍物边界
    }

    enum LineColor
    {
        Unknown = 1, // 未知
        White = 2, // 白色
        Yellow = 3, // 黄色
        Red = 4, // 红色
        Blue = 5, // 蓝色
        Orange = 6, // 橙色
        Green = 7, // 绿色
    }

    class LineSegmentSolidProperty
    {
        public LineColor Color { get; set; } // 线段颜色
        public double? Width { get; set; } // 线段宽度 m
    }

    class LineSegment
    {
        public int ID { get; set; } // 线段ID
        public PointF P1 { get; set; } // 线段首端点的x-y坐标 m
        public float? Z1 { get; set; } // 线段首端点的z坐标 m
        public PointF P2 { get; set; } // 线段末端点的x-y坐标 m
        public float? Z2 { get; set; } // 线段末端点的z坐标 m
        public LineSegmentSolidProperty SolidProperty { get; set; } // 实线段的属性（虚线段则为空）
    }

    class LaneLine
    {
        public int ID { get; set; } // 车道线ID，0表示无效
        public int? RawID { get; set; } // 车道线原始ID
        public double? Confidence { get; set; } // 车道线置信度 %
        public LineDescription Description { get; set; } // 车道线的描述方式

        public LineClass LineClass { get; set; } // Model模式：一致性车道线类型
        public LineColor Color { get; set; } // Model模式：一致性车道线颜色
        public double? Width { get; set; } // Model模式：一致性车道线宽度 m
        public double[] ModelCoefs { get; set; } // Model模式：y = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
        public double[] ModelCoefsZ { get; set; } // Model模式：z = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3，空则表示无效
        public double? RearEnd { get; set; } // Model模式：曲线后端点位置(x轴) m
        public double? FrontEnd { get; set; } // Model模式：曲线前端点位置(x轴) m

        public List<LineSegment> Segments { get; set; } // Segments模式：线段列表

        public bool ModelValid
        {
            get { return Description == LineDescription.Model || Description == LineDescription.Both; }
        }

        public bool SegmentsValid
        {
            get { return Description == LineDescription.Segments || Description == LineDescription.Both; }
        }

        public LaneLine()
        {
            ID = -1;
            Description = LineDescription.Model;
            LineClass = LineClass.General;
            Color = LineColor.Unknown;
            ModelCoefs = new double[] { 0, 0, 0, 0 };
            Segments = new List<LineSegment>();
        }
    }

    class LaneSensorSample : Sample
    {
        // 车道线
        public List<LaneLine> Lines { get; set; } // 车道线列表
        public int? FirstLeftLineIndex { get; set; } // 左侧第一条车道线序号
        public int? FirstRightLineIndex { get; set; } // 右侧第一条车道线序号
        public int? SecondLeftLineIndex { get; set; } // 左侧第二条车道线序号
        public int? SecondRightLineIndex { get; set; } // 右侧第二条车道线序号

        // 本车道行驶状态
        public double? LateralVelocity { get; set; } // 当前车道横向速度 m/s
        public double? CenterDeparture { get; set; } // 当前车道中央偏离距离 m

        // 道路属性部分
        public double? LaneWidth { get; set; } // 当前车道宽度 m
        public double? LaneHeading { get; set; } // 当前车道朝向 deg
        public double? LaneCurvature { get; set; } // 当前车道曲率 1/m

        // 车辆参数部分
        public double? VehicleSpeed { get; set; } // 本车车速 kph
        public double? VehicleCurvature { get; set; } // 本车行驶曲率 1/m
        public double? VehicleAX { get; set; } // 本车纵向加速度 m/s²
        public double? VehicleWidth { get; set; } // 本车宽度 m
        public double? VehicleFrontOverhang { get; set; } // 本车前悬 m
        public double? VehicleWheelBase { get; set; } // 本车轴距 m

        // 车道线识别边界
        public double LinesRearBound { get; set; } // 传感器检测范围后边界 m
        public double LinesFrontBound { get; set; } // 传感器检测范围前边界 m

        public LaneSensorSample()
        {
            Lines = new List<LaneLine>();
            LinesRearBound = 0;
            LinesFrontBound = 30;
        }

        public static string Title
        {
            get
            {
                return "Line count,Segment count,Lateral velocity[m/s],Center departure[m],Lane width[m],Lane heading[°],Lane curvature[1/m],Rear detect bound[m],Front detect bound[m],First left line index,First right line index,Second left line index,Second right line index,Vehicle speed[KPH],Vehicle curvature[1/m],Vehicle acceleration[m/s2],Vehicle width[m],Vehicle overhang[m],Vehicle wheel base[m],Reserved1,Reserved2,Reserved3,Reserved4,First line's ID,Line's raw ID,Line's confidence,Line's description style,Line type,Line color,Line width,Line's y-formula order-0,Line's y-formula order-1,Line's y-formula order-2,Line's y-formula order-3,Line's z-formula vaild,Line's z-formula order-0,Line's z-formula order-1,Line's z-formula order-2,Line's z-formula order-3,Line's rear end,Line's front end,Line's reserved1,Line's reserved2,Line's reserved3,Line DTLC,Line STLC,Line TTLC,Line's reserved4,Line's segment data offset,Line's segment count,Second line's ID / First segment's ID,etc.";
            }
        }

        public static string Protocol
        {
            get
            {
                return "lane-sensor-sample-v4";
            }
        }

        public static string[] Protocols
        {
            get
            {
                return new string[] { "lane-sensor-sample-v2", "lane-sensor-sample-v3", "lane-sensor-sample-v4" };
            }
        }

        public override string GetGeneralSampleProtocol()
        {
            return Protocol;
        }

        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override GeneralSample ToGeneralSample()
        {
            var sample = new GeneralSample();
            sample.Base = Base;
            sample.Offset = Offset;
            sample.Timeline = Timeline;
            sample.Protocol = GetGeneralSampleProtocol();

            int segmentCount = 0;
            var segmentOffset = new int[Lines.Count];
            var segmentSize = new int[Lines.Count];
            for (int i = 0; i < Lines.Count; i++)
            {
                segmentOffset[i] = segmentCount;
                segmentSize[i] = Lines[i].Segments.Count;
                segmentCount += segmentSize[i];
            }

            var data = new GeneralSampleValue[23 + Lines.Count * 27 + segmentCount * 12];
            data[0] = new GeneralSampleValue((double)Lines.Count);
            data[1] = new GeneralSampleValue((double)segmentCount);

            if (LateralVelocity != null) data[2] = new GeneralSampleValue(LateralVelocity.Value);
            if (CenterDeparture != null) data[3] = new GeneralSampleValue(CenterDeparture.Value);
            if (LaneWidth != null) data[4] = new GeneralSampleValue(LaneWidth.Value);
            if (LaneHeading != null) data[5] = new GeneralSampleValue(LaneHeading.Value);
            if (LaneCurvature != null) data[6] = new GeneralSampleValue(LaneCurvature.Value);
            data[7] = new GeneralSampleValue(LinesRearBound);
            data[8] = new GeneralSampleValue(LinesFrontBound);
            if (FirstLeftLineIndex != null) data[9] = new GeneralSampleValue((double)FirstLeftLineIndex.Value);
            if (FirstRightLineIndex != null) data[10] = new GeneralSampleValue((double)FirstRightLineIndex.Value);
            if (SecondLeftLineIndex != null) data[11] = new GeneralSampleValue((double)SecondLeftLineIndex.Value);
            if (SecondRightLineIndex != null) data[12] = new GeneralSampleValue((double)SecondRightLineIndex.Value);
            if (VehicleSpeed != null) data[13] = new GeneralSampleValue(VehicleSpeed.Value);
            if (VehicleCurvature != null) data[14] = new GeneralSampleValue(VehicleCurvature.Value);
            if (VehicleAX != null) data[15] = new GeneralSampleValue(VehicleAX.Value);
            if (VehicleWidth != null) data[16] = new GeneralSampleValue(VehicleWidth.Value);
            if (VehicleFrontOverhang != null) data[17] = new GeneralSampleValue(VehicleFrontOverhang.Value);
            if (VehicleWheelBase != null) data[18] = new GeneralSampleValue(VehicleWheelBase.Value);

            int segBase = 23 + Lines.Count * 27;

            for (int i = 0; i < Lines.Count; i++)
            {
                LaneLine src = Lines[i];
                int baseIndex = 23 + 27 * i;

                data[baseIndex + 0] = new GeneralSampleValue((double)src.ID);
                if (src.RawID != null) data[baseIndex + 1] = new GeneralSampleValue((double)src.RawID.Value);
                if (src.Confidence != null) data[baseIndex + 2] = new GeneralSampleValue(src.Confidence.Value);
                data[baseIndex + 3] = new GeneralSampleValue((double)(uint)src.Description);

                if (src.ModelValid)
                {
                    data[baseIndex + 4] = new GeneralSampleValue((double)(uint)src.LineClass);
                    data[baseIndex + 5] = new GeneralSampleValue((double)(uint)src.Color);
                    if (src.Width != null) data[baseIndex + 6] = new GeneralSampleValue(src.Width.Value);
                    data[baseIndex + 7] = new GeneralSampleValue(src.ModelCoefs[0]);
                    data[baseIndex + 8] = new GeneralSampleValue(src.ModelCoefs[1]);
                    data[baseIndex + 9] = new GeneralSampleValue(src.ModelCoefs[2]);
                    data[baseIndex + 10] = new GeneralSampleValue(src.ModelCoefs[3]);
                    if (src.ModelCoefsZ == null) data[baseIndex + 11] = new GeneralSampleValue(0.0);
                    else
                    {
                        data[baseIndex + 11] = new GeneralSampleValue(1.0);
                        data[baseIndex + 12] = new GeneralSampleValue(src.ModelCoefsZ[0]);
                        data[baseIndex + 13] = new GeneralSampleValue(src.ModelCoefsZ[1]);
                        data[baseIndex + 14] = new GeneralSampleValue(src.ModelCoefsZ[2]);
                        data[baseIndex + 15] = new GeneralSampleValue(src.ModelCoefsZ[3]);
                    }
                    if (src.RearEnd != null) data[baseIndex + 16] = new GeneralSampleValue(src.RearEnd.Value);
                    if (src.FrontEnd != null) data[baseIndex + 17] = new GeneralSampleValue(src.FrontEnd.Value);
                }

                if (src.SegmentsValid)
                {
                    var segOffset = segmentOffset[i];
                    var segSize = segmentSize[i];
                    data[baseIndex + 25] = new GeneralSampleValue((double)segOffset);
                    data[baseIndex + 26] = new GeneralSampleValue((double)segSize);
                    if (segSize > 0)
                    {
                        for (int n = 0; n < segSize; n++)
                        {
                            var seg = src.Segments[n];
                            int segBaseIndex = segBase + 12 * (segOffset + n);

                            data[segBaseIndex + 0] = new GeneralSampleValue((double)seg.ID);
                            data[segBaseIndex + 1] = new GeneralSampleValue(seg.P1.X);
                            data[segBaseIndex + 2] = new GeneralSampleValue(seg.P1.Y);
                            if (seg.Z1 != null) data[segBaseIndex + 3] = new GeneralSampleValue(seg.Z1.Value);
                            data[segBaseIndex + 4] = new GeneralSampleValue(seg.P2.X);
                            data[segBaseIndex + 5] = new GeneralSampleValue(seg.P2.Y);
                            if (seg.Z2 != null) data[segBaseIndex + 6] = new GeneralSampleValue(seg.Z2.Value);
                            if (seg.SolidProperty == null) data[segBaseIndex + 7] = new GeneralSampleValue(0.0);
                            else
                            {
                                data[segBaseIndex + 7] = new GeneralSampleValue(1.0);
                                data[segBaseIndex + 8] = new GeneralSampleValue((double)(uint)seg.SolidProperty.Color);
                                if (seg.SolidProperty.Width != null) data[segBaseIndex + 9] = new GeneralSampleValue(seg.SolidProperty.Width.Value);
                            }
                        }
                    }
                }

                var dtlc = GetDTLC(src);
                if (dtlc != null) data[baseIndex + 21] = new GeneralSampleValue(dtlc.Value);
                var stlc = GetSTLC(src);
                if (stlc != null) data[baseIndex + 22] = new GeneralSampleValue(stlc.Value);
                var ttlc = GetTTLC(src);
                if (ttlc != null) data[baseIndex + 23] = new GeneralSampleValue(ttlc.Value);
            }

            sample.Values = data.ToList();
            sample.NumberOfSignificants = sample.Values.Count;

            return sample;
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            #region lane-sensor-sample-v4
            if (sample.Protocol == "lane-sensor-sample-v4")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 23) return false;
                if (v[0].IsNotNumber() || v[1].IsNotNumber()) return false;

                int lineCount = (int)v[0].number;
                int segmentCount = (int)v[1].number;
                if (v.Length != 23 + lineCount * 27 + segmentCount * 12) return false;

                LateralVelocity = v[2].ToDouble();
                CenterDeparture = v[3].ToDouble();
                LaneWidth = v[4].ToDouble();
                LaneHeading = v[5].ToDouble();
                LaneCurvature = v[6].ToDouble();
                LinesRearBound = v[7].number;
                LinesFrontBound = v[8].number;
                FirstLeftLineIndex = v[9].ToInt();
                FirstRightLineIndex = v[10].ToInt();
                SecondLeftLineIndex = v[11].ToInt();
                SecondRightLineIndex = v[12].ToInt();
                VehicleSpeed = v[13].ToDouble();
                VehicleCurvature = v[14].ToDouble();
                VehicleAX = v[15].ToDouble();
                VehicleWidth = v[16].ToDouble();
                VehicleFrontOverhang = v[17].ToDouble();
                VehicleWheelBase = v[18].ToDouble();

                int segBase = 23 + lineCount * 27;

                Lines.Clear();
                for (int i = 0; i < lineCount; i++)
                {
                    var line = new LaneLine();
                    int baseIndex = 23 + 27 * i;

                    line.ID = (int)v[baseIndex + 0].number;
                    line.RawID = v[baseIndex + 1].ToInt();
                    line.Confidence = v[baseIndex + 2].ToDouble();
                    line.Description = (LineDescription)(int)v[baseIndex + 3].number;

                    if (line.ModelValid)
                    {
                        line.LineClass = (LineClass)(int)v[baseIndex + 4].number;
                        line.Color = (LineColor)(int)v[baseIndex + 5].number;
                        line.Width = v[baseIndex + 6].ToDouble();
                        line.ModelCoefs[0] = v[baseIndex + 7].number;
                        line.ModelCoefs[1] = v[baseIndex + 8].number;
                        line.ModelCoefs[2] = v[baseIndex + 9].number;
                        line.ModelCoefs[3] = v[baseIndex + 10].number;
                        if (v[baseIndex + 11].number == 1)
                        {
                            line.ModelCoefsZ = new double[4];
                            line.ModelCoefsZ[0] = v[baseIndex + 12].number;
                            line.ModelCoefsZ[1] = v[baseIndex + 13].number;
                            line.ModelCoefsZ[2] = v[baseIndex + 14].number;
                            line.ModelCoefsZ[3] = v[baseIndex + 15].number;
                        }
                        line.RearEnd = v[baseIndex + 16].ToDouble();
                        line.FrontEnd = v[baseIndex + 17].ToDouble();
                    }

                    if (line.SegmentsValid)
                    {
                        int segOffset = (int)v[baseIndex + 25].number;
                        int segSize = (int)v[baseIndex + 26].number;
                        if (segSize > 0)
                        {
                            for (int n = 0; n < segSize; n++)
                            {
                                var seg = new LineSegment();
                                int segBaseIndex = segBase + 12 * (segOffset + n);

                                seg.ID = (int)v[segBaseIndex + 0].number;
                                seg.P1 = new PointF((float)v[segBaseIndex + 1].number, (float)v[segBaseIndex + 2].number);
                                seg.Z1 = v[segBaseIndex + 3].ToFloat();
                                seg.P2 = new PointF((float)v[segBaseIndex + 4].number, (float)v[segBaseIndex + 5].number);
                                seg.Z2 = v[segBaseIndex + 6].ToFloat();
                                if (v[segBaseIndex + 7].number == 1)
                                {
                                    seg.SolidProperty = new LineSegmentSolidProperty();
                                    seg.SolidProperty.Color = (LineColor)(int)v[segBaseIndex + 8].number;
                                    seg.SolidProperty.Width = v[segBaseIndex + 9].ToDouble();
                                }

                                line.Segments.Add(seg);
                            }
                        }
                    }

                    Lines.Add(line);
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }
            #endregion

            #region lane-sensor-sample-v3
            else if (sample.Protocol == "lane-sensor-sample-v3")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 17) return false;
                if (v[0].IsNotNumber()) return false;

                int lineCount = (int)v[0].number;
                if (v.Length != 17 + lineCount * 15) return false;

                LateralVelocity = v[1].ToDouble();
                CenterDeparture = v[2].ToDouble();
                LaneWidth = v[3].ToDouble();
                LaneHeading = v[4].ToDouble();
                LaneCurvature = v[5].ToDouble();
                VehicleSpeed = v[6].ToDouble();
                VehicleCurvature = v[7].ToDouble();
                VehicleAX = v[8].ToDouble();
                VehicleWidth = v[9].ToDouble();
                VehicleFrontOverhang = v[10].ToDouble();
                LinesRearBound = v[11].number;
                LinesFrontBound = v[12].number;
                FirstLeftLineIndex = v[13].ToInt();
                FirstRightLineIndex = v[14].ToInt();
                SecondLeftLineIndex = v[15].ToInt();
                SecondRightLineIndex = v[16].ToInt();

                Lines.Clear();
                for (int i = 0; i < lineCount; i++)
                {
                    var line = new LaneLine();
                    int baseIndex = 17 + 15 * i;

                    line.ID = (int)v[baseIndex + 0].number;
                    line.RawID = v[baseIndex + 1].ToInt();
                    line.LineClass = (LineClass)(int)v[baseIndex + 2].number;
                    line.Color = (LineColor)(int)v[baseIndex + 3].number;
                    line.Confidence = v[baseIndex + 4].ToDouble();
                    line.Width = v[baseIndex + 5].ToDouble();
                    line.ModelCoefs[0] = v[baseIndex + 6].number;
                    line.ModelCoefs[1] = v[baseIndex + 7].number;
                    line.ModelCoefs[2] = v[baseIndex + 8].number;
                    line.ModelCoefs[3] = v[baseIndex + 9].number;
                    line.RearEnd = v[baseIndex + 10].ToDouble();
                    line.FrontEnd = v[baseIndex + 11].ToDouble();

                    Lines.Add(line);
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }
            #endregion

            #region lane-sensor-sample-v2
            else if (sample.Protocol == "lane-sensor-sample-v2")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 30) return false;

                var leftDetected = v[0].number == 1;
                var rightDetected = v[1].number == 1;

                Lines.Clear();

                int index = 0;
                if (leftDetected)
                {
                    var newLine = new LaneLine();

                    newLine.ModelCoefs = new double[4]
                    {
                        v[2].number,
                        v[3].number,
                        v[4].number,
                        v[5].number,
                    };
                    newLine.LineClass = (LineClass)v[6].number;

                    Lines.Add(newLine);
                    FirstLeftLineIndex = index++;
                }
                if (rightDetected)
                {
                    var newLine = new LaneLine();

                    newLine.ModelCoefs = new double[4]
                    {
                        v[8].number,
                        v[9].number,
                        v[10].number,
                        v[11].number,
                    };
                    newLine.LineClass = (LineClass)v[12].number;

                    Lines.Add(newLine);
                    FirstRightLineIndex = index++;
                }

                LaneWidth = v[14].ToDouble();
                LaneHeading = v[15].ToDouble();
                LaneCurvature = v[16].ToDouble();

                LateralVelocity = v[23].ToDouble();
                CenterDeparture = v[24].ToDouble();

                VehicleSpeed = v[25].ToDouble();
                VehicleCurvature = v[26].ToDouble();
                VehicleAX = v[27].ToDouble();
                VehicleWidth = v[28].ToDouble();
                VehicleFrontOverhang = v[29].ToDouble();

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }
            #endregion

            return false;
        }

        public override bool SupportInterpolation()
        {
            return true;
        }

        protected override Sample Interpolate(SearchResult input)
        {
            var s1 = input.s1 as LaneSensorSample;
            var s2 = input.s2 as LaneSensorSample;
            var w1 = input.w1;
            var w2 = input.w2;

            bool noID = false;
            foreach (var line in s1.Lines)
            {
                if (line.ID <= 0)
                {
                    noID = true;
                    break;
                }
            }
            foreach (var line in s2.Lines)
            {
                if (line.ID <= 0)
                {
                    noID = true;
                    break;
                }
            }

            if (noID)
            {
                return w1 > w2 ? s1 : s2;
            }

            int? firstLeftID = null, firstRightID = null;
            if (s1.FirstLeftLineIndex != null && s2.FirstLeftLineIndex != null &&
                s1.Lines[s1.FirstLeftLineIndex.Value].ID == s2.Lines[s2.FirstLeftLineIndex.Value].ID)
            {
                firstLeftID = s1.Lines[s1.FirstLeftLineIndex.Value].ID;
            }
            if (s1.FirstRightLineIndex != null && s2.FirstRightLineIndex != null &&
                s1.Lines[s1.FirstRightLineIndex.Value].ID == s2.Lines[s2.FirstRightLineIndex.Value].ID)
            {
                firstRightID = s1.Lines[s1.FirstRightLineIndex.Value].ID;
            }

            int? secondLeftID = null, secondRightID = null;
            if (s1.SecondLeftLineIndex != null && s2.SecondLeftLineIndex != null &&
                s1.Lines[s1.SecondLeftLineIndex.Value].ID == s2.Lines[s2.SecondLeftLineIndex.Value].ID)
            {
                secondLeftID = s1.Lines[s1.SecondLeftLineIndex.Value].ID;
            }
            if (s1.SecondRightLineIndex != null && s2.SecondRightLineIndex != null &&
                s1.Lines[s1.SecondRightLineIndex.Value].ID == s2.Lines[s2.SecondRightLineIndex.Value].ID)
            {
                secondRightID = s1.Lines[s1.SecondRightLineIndex.Value].ID;
            }

            var output = new LaneSensorSample();

            output.LateralVelocity = s1.LateralVelocity * w1 + s2.LateralVelocity * w2;
            output.CenterDeparture = s1.CenterDeparture * w1 + s2.CenterDeparture * w2;

            output.LaneWidth = s1.LaneWidth * w1 + s2.LaneWidth * w2;
            output.LaneCurvature = s1.LaneCurvature * w1 + s2.LaneCurvature * w2;
            output.LaneHeading = s1.LaneHeading * w1 + s2.LaneHeading * w2;

            output.VehicleSpeed = s1.VehicleSpeed * w1 + s2.VehicleSpeed * w2;
            output.VehicleCurvature = s1.VehicleCurvature * w1 + s2.VehicleCurvature * w2;
            output.VehicleAX = s1.VehicleAX * w1 + s2.VehicleAX * w2;
            output.VehicleWidth = s1.VehicleWidth;
            output.VehicleFrontOverhang = s1.VehicleFrontOverhang;
            output.VehicleWheelBase = s1.VehicleWheelBase;

            output.LinesRearBound = w1 > w2 ? s1.LinesRearBound : s2.LinesRearBound;
            output.LinesFrontBound = w1 > w2 ? s1.LinesFrontBound : s2.LinesFrontBound;

            var lines = new List<LaneLine>();
            int index = 0;
            for (int i = 0; i < s1.Lines.Count; i++)
            {
                var s1Line = s1.Lines[i];
                for (int j = 0; j < s2.Lines.Count; j++)
                {
                    var s2Line = s2.Lines[j];

                    if (s1Line.ID == s2Line.ID && s1Line.Description == s2Line.Description)
                    {
                        if (firstLeftID != null && firstLeftID.Value == s1Line.ID) output.FirstLeftLineIndex = index;
                        if (firstRightID != null && firstRightID.Value == s1Line.ID) output.FirstRightLineIndex = index;
                        if (secondLeftID != null && secondLeftID.Value == s1Line.ID) output.SecondLeftLineIndex = index;
                        if (secondRightID != null && secondRightID.Value == s1Line.ID) output.SecondRightLineIndex = index;

                        var line = new LaneLine();
                        line.ID = s1Line.ID;
                        line.RawID = s1Line.RawID;
                        line.Confidence = s1Line.Confidence * w1 + s2Line.Confidence * w2;
                        line.Description = s1Line.Description;

                        if (line.ModelValid)
                        {
                            line.LineClass = w1 > w2 ? s1Line.LineClass : s2Line.LineClass;
                            line.Color = w1 > w2 ? s1Line.Color : s2Line.Color;
                            line.Width = s1Line.Width * w1 + s2Line.Width * w2;
                            line.ModelCoefs[0] = s1Line.ModelCoefs[0] * w1 + s2Line.ModelCoefs[0] * w2;
                            line.ModelCoefs[1] = s1Line.ModelCoefs[1] * w1 + s2Line.ModelCoefs[1] * w2;
                            line.ModelCoefs[2] = s1Line.ModelCoefs[2] * w1 + s2Line.ModelCoefs[2] * w2;
                            line.ModelCoefs[3] = s1Line.ModelCoefs[3] * w1 + s2Line.ModelCoefs[3] * w2;
                            if (s1Line.ModelCoefsZ != null && s2Line.ModelCoefsZ != null)
                            {
                                line.ModelCoefsZ = new double[4];
                                line.ModelCoefsZ[0] = s1Line.ModelCoefsZ[0] * w1 + s2Line.ModelCoefsZ[0] * w2;
                                line.ModelCoefsZ[1] = s1Line.ModelCoefsZ[1] * w1 + s2Line.ModelCoefsZ[1] * w2;
                                line.ModelCoefsZ[2] = s1Line.ModelCoefsZ[2] * w1 + s2Line.ModelCoefsZ[2] * w2;
                                line.ModelCoefsZ[3] = s1Line.ModelCoefsZ[3] * w1 + s2Line.ModelCoefsZ[3] * w2;
                            }
                            line.RearEnd = s1Line.RearEnd * w1 + s2Line.RearEnd * w2;
                            line.FrontEnd = s1Line.FrontEnd * w1 + s2Line.FrontEnd * w2;
                        }

                        if (line.SegmentsValid)
                        {
                            for (int si = 0; si < s1Line.Segments.Count; si++)
                            {
                                var s1Seg = s1Line.Segments[si];
                                for (int sj = 0; sj < s2Line.Segments.Count; sj++)
                                {
                                    var s2Seg = s2Line.Segments[sj];

                                    if (s1Seg.ID == s2Seg.ID)
                                    {
                                        var seg = new LineSegment();
                                        seg.ID = s1Seg.ID;
                                        seg.P1 = new PointF((float)(s1Seg.P1.X * w1 + s2Seg.P1.X * w2), (float)(s1Seg.P1.Y * w1 + s2Seg.P1.Y * w2));
                                        seg.Z1 = s1Seg.Z1 * (float)w1 + s2Seg.Z1 * (float)w2;
                                        seg.P2 = new PointF((float)(s1Seg.P2.X * w1 + s2Seg.P2.X * w2), (float)(s1Seg.P2.Y * w1 + s2Seg.P2.Y * w2));
                                        seg.Z2 = s1Seg.Z2 * (float)w1 + s2Seg.Z2 * (float)w2;
                                        seg.SolidProperty = w1 > w2 ? s1Seg.SolidProperty : s2Seg.SolidProperty;
                                        line.Segments.Add(seg);
                                    }
                                }
                            }
                        }

                        lines.Add(line);
                        index++;
                        break;
                    }
                }
            }
            output.Lines = lines;

            return output;
        }

        public double? GetDTLC(LaneLine line)
        {
            if (line.ModelValid && VehicleWidth != null && VehicleFrontOverhang != null)
            {
                var values = line.ModelCoefs;
                bool isLeft = values[0] > 0;

                double x = -1 * VehicleFrontOverhang.Value;
                double y = (values[3] * x * x * x + values[2] * x * x
                    + values[1] * x + values[0]) * (isLeft ? 1 : -1);
                double cosTheta = Math.Abs(Math.Cos(Math.Atan(values[1])));
                return cosTheta * (y - VehicleWidth.Value / 2.0);
            }
            else return null;
        }

        public double? GetSTLC(LaneLine line)
        {
            if (line.ModelValid && VehicleSpeed != null)
            {
                var values = line.ModelCoefs;
                bool isLeft = values[0] > 0;

                double curv = VehicleCurvature == null ? 0 : VehicleCurvature.Value;
                double fo = VehicleFrontOverhang == null ? 0.9 : VehicleFrontOverhang.Value;
                double wb = VehicleWheelBase == null ? 2.8 : VehicleWheelBase.Value;

                double sinTheta = Math.Sin(Math.Atan(values[1] - curv * (fo + wb)));
                return (isLeft ? -1.0 : 1.0) * sinTheta * VehicleSpeed.Value / 3.6;
            }
            else return null;
        }

        public double? GetTTLC(LaneLine line)
        {
            if (line.ModelValid && VehicleSpeed != null && VehicleWidth != null && VehicleFrontOverhang != null)
            {
                if (VehicleSpeed.Value <= 0) return null;

                var values = line.ModelCoefs;
                bool isLeft = values[0] > 0;

                double wb = VehicleWheelBase != null ? VehicleWheelBase.Value : 2.8;
                double x0 = -VehicleFrontOverhang.Value - wb;
                double x = x0;
                double y = 0;
                double vx = VehicleSpeed.Value / 3.6;

                var loopTimes = vx < 5 ? 1000 : (int)(50.0 / vx * 100); // 超过50米越线则不输出（精度无法保证）

                double rx = wb;
                double ry = (isLeft ? 0.5 : -0.5) * VehicleWidth.Value;

                for (int i = 0; i <= loopTimes; i++)
                {
                    double k = 0;
                    if (VehicleCurvature != null) k = VehicleCurvature.Value * (x - x0);

                    double cosa = 1.0 / Math.Abs(1 + k);
                    double sina = cosa * k;

                    double tx = x + cosa * rx - sina * ry;
                    double ty = y + sina * rx + cosa * ry;
                    double ly = values[3] * tx * tx * tx + values[2] * tx * tx + values[1] * tx + values[0];

                    bool passed = false;
                    if (isLeft)
                    {
                        if (ly <= ty) passed = true;
                    }
                    else
                    {
                        if (ly >= ty) passed = true;
                    }
                    if (passed)
                    {
                        if (i == 0) return null;
                        else return (double)i / 100;
                    }

                    if (VehicleAX != null) vx += VehicleAX.Value * 0.01;
                    if (vx <= 0) break;

                    x += vx * 0.01 * cosa;
                    y += vx * 0.01 * sina;
                }

                return null;
            }
            else return null;
        }
    }
}
